// --------------------------------------------------------------------
// Copyright (c) 2010 by Terasic Technologies Inc. 
// --------------------------------------------------------------------
//
// Permission:
//
//   Terasic grants permission to use and modify this code for use
//   in synthesis for all Terasic Development Boards and Altera Development 
//   Kits made by Terasic.  Other use of this code, including the selling 
//   ,duplication, or modification of any portion is strictly prohibited.
//
// Disclaimer:
//
//   This VHDL/Verilog or C/C++ source code is intended as a design reference
//   which illustrates how these types of functions can be implemented.
//   It is the user's responsibility to verify their design for
//   consistency and functionality through the use of formal
//   verification methods.  Terasic provides no warranty regarding the use 
//   or functionality of this code.
//
// --------------------------------------------------------------------
//           
//                     Terasic Technologies Inc
//                     356 Fu-Shin E. Rd Sec. 1. JhuBei City,
//                     HsinChu County, Taiwan
//                     302
//
//                     web: http://www.terasic.com/
//                     email: support@terasic.com
//
// --------------------------------------------------------------------

#include "terasic_includes.h"
#include "accelerometer_adxl345_spi.h"
#include "flash.h"
#include "adc_spi_read.h"
#include "I2C.h"
#include "sys/alt_irq.h"
#include "altera_avalon_timer_regs.h"

//Keith Define Start
// SPI
#define SPI_OFFSET_START 0  
#define SPI_OFFSET_READ_BYTE 1
#define SPI_OFFSET_FIFO 2
#define SPI_OFFSET_CE 3

volatile char * volatile SPI_BASE_PTX = (volatile char * volatile) SPI_SLAVE_RF_BASE;
int SPI_PTX_IRQ = SPI_SLAVE_RF_IRQ;

// system flags
short FLAG_RF_PTX_READY = 2;
char status_register = 0;

// rf_ptx_buffer
char rf_ptx_buffer[64];

// function prototypes
static void irq_rf_ptx_rx(void *context, alt_u32 id);
void spi_send_cmd(volatile char * volatile SPI_BASE, char *cmd, char num_cmd, char num_respond);
void spi_set_reg(volatile char * volatile SPI_BASE, char addr, char* value, char num);
void spi_read_reg(volatile char * volatile SPI_BASE, char addr, char *buffer, char bytes);
void rf_initialize_tx();
void rf_ptx_tx(const char const *payload);
void rf_print_fifo_info(volatile char * volatile SPI_BASE);
//Keith Define End


#define GYRO 0x68         // gyro I2C address
#define REG_GYRO_X 0x1D   // IMU-3000 Register address for GYRO_XOUT_H
#define ACCEL 0x53        // Accel I2c Address
#define ADXL345_POWER_CTL 0x2D
/*
void DEMO_ACCELEROMETER(void);
void DEMO_ADC(void);
void DEMO_I2C(void);
void DEMO_EPCS(void);*/
void init_sensors(void);
int read_sensors(void);

typedef enum{
    T_EERPOM,
    T_EPCS,
    T_GSENSOR,
    T_ADC,
}LOCAL_TEST_ITEM;

// Keith Functions Start
static void irq_rf_ptx_rx(void *context, alt_u32 id)
{

    // save ce status
    char ce_status = *(SPI_BASE_PTX + SPI_OFFSET_CE);

    // disable chip;
    *(SPI_BASE_PTX + SPI_OFFSET_CE) = 0x00;

    char buffer[128];

    // read payload width
    buffer[0] = 0x60;
    spi_send_cmd(SPI_BASE_PTX, buffer,1,2);
    char payload_length = buffer[1];
    while (payload_length == 0)
    {
        spi_send_cmd(SPI_BASE_PTX, buffer,1,2);
        payload_length = buffer[1];
        rf_print_fifo_info(SPI_BASE_PTX);
    }
    // read payload
    buffer[0] = 0x61;
    spi_send_cmd(SPI_BASE_PTX, buffer,1,payload_length+1);

    strcpy(rf_ptx_buffer, &buffer[1]);

    status_register |= 1 << FLAG_RF_PTX_READY;

    // remove irq tag
    buffer[0] = 0x40;
    spi_set_reg(SPI_BASE_PTX, 0x07,buffer,1);

    *(SPI_BASE_PTX + SPI_OFFSET_CE) = ce_status;
}

void spi_send_cmd(volatile char * volatile SPI_BASE, char *cmd, char num_cmd, char num_respond)
{
    char i;
    for (i = 0; i < num_cmd; i++)
    {
        *(SPI_BASE + SPI_OFFSET_FIFO) = *(cmd + i);
    }

    *(SPI_BASE + SPI_OFFSET_READ_BYTE) = num_respond;
    *(SPI_BASE + SPI_OFFSET_START) = 0;
    char dummy;
    for (i = 0; i < num_respond; i++)
    {
        dummy = *(SPI_BASE + 0);
        *(cmd + i) = dummy;
    }
}

void spi_set_reg(volatile char * volatile SPI_BASE, char addr, char* value, char num)
{

    // transmit
    char write_addr = addr + 0x20;
    *(SPI_BASE + SPI_OFFSET_FIFO) = write_addr;

    char i;
    for (i = 0; i <num; i++)
    {
        // LSB first
        *(SPI_BASE + SPI_OFFSET_FIFO) = *(value + num - (char)1 - i);
    }
        
    *(SPI_BASE + SPI_OFFSET_READ_BYTE) = 0x01;
    *(SPI_BASE + SPI_OFFSET_START) = 0;
    char dummy = *(SPI_BASE + 0); 

    if (addr != 0x07)
    {
        // read back to check;
        *(SPI_BASE + SPI_OFFSET_FIFO) = addr;
        *(SPI_BASE + SPI_OFFSET_READ_BYTE) = num + 1;
        *(SPI_BASE + SPI_OFFSET_START) = 0;
    
        dummy = *(SPI_BASE + 0); 
        for (i = 0; i < num; i ++)
        {
             
            dummy = *(SPI_BASE + 0); 
            if (dummy != *(value + num - 1-i))
            {
                char expected = *(value + num - 1-i);
                /*uart_send_str("ERROR: register not set correctly: "); 
                if (SPI_BASE == SPI_BASE_PTX)
                {
                    uart_send_str ("|PTX| ");
                }
                uart_send_str("SET:|");
                uart_send_int(expected);
                uart_send_str("| GET:|");
                uart_send_int(dummy);
                uart_send_str("|\n");*/
            }
        }
    }
}

void spi_read_reg(volatile char * volatile SPI_BASE, char addr, char *buffer, char bytes)
{
    *(SPI_BASE + SPI_OFFSET_FIFO) = addr;
    *(SPI_BASE + SPI_OFFSET_READ_BYTE) = bytes + 1;
    *(SPI_BASE + SPI_OFFSET_START) = 0;

    *(buffer + bytes) = 0;
    
    char dummy = *(SPI_BASE);
    char i;
    for (i = 0; i < bytes; i++)
    {
        dummy = *(SPI_BASE);
        *(buffer + bytes - 1 - i) = dummy;
    }
}

void rf_print_fifo_info(volatile char * volatile SPI_BASE)
{
    char result;
    spi_read_reg(SPI_BASE, 0x17, &result, 1);

   /* uart_send_str("INFO: TX_FULL: ");
    if (result & 0x20)
    {
        uart_send_str("TRUE\n");
    }
    else
    {
        uart_send_str("FALSE\n");
    }

    uart_send_str("INFO: TX_EMPTY: ");
    if (result & 0x10)
    {
        uart_send_str("TRUE\n");
    }
    else
    {
        uart_send_str("FALSE\n");
    }

    uart_send_str("INFO: RX_FULL: ");
    if (result & 0x02)
    {
        uart_send_str("TRUE\n");
    }
    else
    {
        uart_send_str("FALSE\n");
    }

    uart_send_str("INFO: RX_EMPTY: ");
    if (result & 0x01)
    {
        uart_send_str("TRUE\n");
    }
    else
    {
        uart_send_str("FALSE\n");
    }*/
}
    
void rf_initialize_tx()
{
    char buffer[64];
    char addr, num;

    *(SPI_BASE_PTX + SPI_OFFSET_CE) = 0x00;

    // config register
    addr = 0x00;    
    buffer[0]=0x3e;
    num = 1;
    spi_set_reg(SPI_BASE_PTX, addr,buffer,num);

    // SETUP_RETER register
    addr = 0x04;    
    buffer[0]=0x13;
    num = 1;
    spi_set_reg(SPI_BASE_PTX, addr,buffer,num);


    addr = 0x07;    
    buffer[0]=0x70;
    num = 1;
    spi_set_reg(SPI_BASE_PTX, addr,buffer,num);


    // payload width
    addr = 0x11;    
    buffer[0]=0x10;
    num = 1;
    spi_set_reg(SPI_BASE_PTX, addr,buffer,num);


    // enable dynamic payload length for datalink0
    addr = 0x1c;
    buffer[0] = 0x01;
    num = 1;
    spi_set_reg(SPI_BASE_PTX, addr,buffer,num);

    // enable dynamic payload length 
    addr = 0x1d;
    buffer[0] = 0x06;
    num = 1;
    spi_set_reg(SPI_BASE_PTX, addr,buffer,num);


    // issue activate command
    buffer[0] = 0x50;
    buffer[1] = 0x73;
    spi_send_cmd(SPI_BASE_PTX, buffer,2,1);

    // flush rx tx fifo
    buffer[0] = 0xe1;
    spi_send_cmd(SPI_BASE_PTX, buffer,1,1);
    buffer[0] = 0xe2;
    spi_send_cmd(SPI_BASE_PTX, buffer,1,1);

    *(SPI_BASE_PTX + SPI_OFFSET_CE) = 0x00;
}

void rf_ptx_flush_tx_fifo()
{
    char buffer[10];
    // flush rx tx fifo
    buffer[0] = 0xe1;
    spi_send_cmd(SPI_BASE_PTX, buffer,1,1);
    buffer[0] = 0xe2;
    spi_send_cmd(SPI_BASE_PTX, buffer,1,1);
    buffer[0] = 0x10;

}

// send a string through tx module
// please note the string has to be c style string(null terminated)
void rf_ptx_tx(const char const * payload_input)
{

    char * payload = payload_input;
    char buffer[64];
    short length = strlen(payload);
    char * err = "200:ERROR:OVERFLOW\n";
    if (length >= 32)
    {
        //uart_send_str
        printf(err);
        payload=err;
        length = strlen(err);
    }
    buffer[0] = 0xA0;
    strcpy(&buffer[1], payload);
    spi_send_cmd(SPI_BASE_PTX, buffer,length+2,1);
    *(SPI_BASE_PTX + SPI_OFFSET_CE) = 1;

    int i;
    for (i=0; i< 60; i++)
    {
    }

    *(SPI_BASE_PTX + SPI_OFFSET_CE) = 0;

    buffer[0] = 0xff;
    spi_send_cmd(SPI_BASE_PTX,buffer,1,1);
    bool sent = buffer[0] & 0x20;
    bool failed = buffer[0] & 0x10;  
    
    while(sent == 0 && failed == 0)
    {   buffer[0] = 0xff;
        spi_send_cmd(SPI_BASE_PTX,buffer,1,1);
        sent = buffer[0] & 0x20;
        failed = buffer[0] & 0x10;  
    

        if (sent)
        {
            buffer[0] = 0x20;
            spi_set_reg(SPI_BASE_PTX, 0x07,buffer,1);
        
        }
        if (failed)
        {
            buffer[0] = 0x10;
            spi_set_reg(SPI_BASE_PTX, 0x07,buffer,1);
            rf_ptx_flush_tx_fifo();
        }
    
    }
}


// Keith Functions End

bool bKeyPressed = FALSE;

void KEY_ISR(void* context, alt_u32 id){
    if (id == KEY_IRQ){
        bKeyPressed = TRUE;
        
        // clear interrupt flag
        IOWR_ALTERA_AVALON_PIO_EDGE_CAP(KEY_BASE,0);
    }    
}

void EnableKeyInterrupt(void){
    int error;
    
    // enable interrupt, 2-keybutton
    //IOWR_ALTERA_AVALON_PIO_IRQ_MASK(KEY_BASE,0x03);
    // enable interrupt, 1-keybutton
    IOWR_ALTERA_AVALON_PIO_IRQ_MASK(KEY_BASE,0x01);
    
    // clear capture flag
    IOWR_ALTERA_AVALON_PIO_EDGE_CAP(KEY_BASE,0);
    //

    bKeyPressed = FALSE;
    // register interrupt isr    
    error = alt_irq_register (KEY_IRQ, 0, KEY_ISR);
    if (error)
        printf("Failed to register interrut\r\n");
    
}
void handle_timer_interrupt (void* context, alt_u32 id){
     
   /* 
        if(timer_counter == 0 ){
            IOWR_ALTERA_AVALON_PIO_DATA(MOTOR_0_BASE, 1);
            IOWR_ALTERA_AVALON_PIO_DATA(MOTOR_1_BASE, 1);
            IOWR_ALTERA_AVALON_PIO_DATA(MOTOR_2_BASE, 1);
            IOWR_ALTERA_AVALON_PIO_DATA(MOTOR_3_BASE, 1);
        }
        else if (timer_counter == motor_0_on_width ){
             IOWR_ALTERA_AVALON_PIO_DATA(MOTOR_0_BASE, 0);
        }
        else if (timer_counter == motor_1_on_width ){
             IOWR_ALTERA_AVALON_PIO_DATA(MOTOR_1_BASE, 0);
        }
        else if (timer_counter == motor_2_on_width ){
             IOWR_ALTERA_AVALON_PIO_DATA(MOTOR_2_BASE, 0);
        }
        else if (timer_counter == motor_3_on_width ){
             IOWR_ALTERA_AVALON_PIO_DATA(MOTOR_3_BASE, 0);
        }
        if(timer_counter == ONE_MS ){
            timer_counter == 0;
        }
        timer_counter ++;*/
         printf("I'm in anyways\n");
        if (id == TIMER_IRQ){
            printf("I'm in\n");
            IOWR_ALTERA_AVALON_TIMER_STATUS(TIMER_BASE, 0);//clear TO

    } 
       
   
}
int main(void)
{
    int error;
    int timer_counter;
    int status, control;

  timer_counter = 0;  
    printf("DE-Nano Demo\r\n");
     status = IORD_ALTERA_AVALON_TIMER_STATUS(TIMER_BASE);
        printf("initial status: %x\n", status);
        control = IORD_ALTERA_AVALON_TIMER_CONTROL(TIMER_BASE);
        printf("initial control: %x\n", control);
    // enble key interrupt
    //EnableKeyInterrupt();  
    
    error = alt_irq_register (TIMER_IRQ, 0, handle_timer_interrupt);
    IOWR_ALTERA_AVALON_TIMER_STATUS( TIMER_BASE, 0 ); 
    
    //IOWR_ALTERA_AVALON_TIMER_PERIODL(TIMER_BASE, 20000 );
   // IOWR_ALTERA_AVALON_TIMER_PERIODH(TIMER_BASE, 0x0);

     IOWR_ALTERA_AVALON_TIMER_PERIODL( TIMER_BASE,  (1000 & 0xFFFF) ); 
     IOWR_ALTERA_AVALON_TIMER_PERIODH( TIMER_BASE,  ((1000>>16) & 0xFFFF) ); 
     
     
    IOWR_ALTERA_AVALON_TIMER_CONTROL( TIMER_BASE, 0x7 ); 
     while(1 ) {
        if( (IORD_ALTERA_AVALON_TIMER_STATUS( TIMER_BASE ) & ALTERA_AVALON_TIMER_STATUS_TO_MSK) == 1){
            if(timer_counter == 100 ){
                printf("hehe %d\n",IORD_ALTERA_AVALON_TIMER_STATUS( TIMER_BASE ));
                timer_counter = 0;
                
            }
            
            
            
            IOWR_ALTERA_AVALON_TIMER_STATUS( TIMER_BASE, 0 ); 
            
            timer_counter++;
          }
     }
    
    
    
     if (error)
        printf("Failed to timer interrut\r\n");
     usleep(100);
     printf("haha\n");



    //enable rf interrupt
    alt_irq_register(SPI_PTX_IRQ, 0, irq_rf_ptx_rx);
    rf_initialize_tx();
    
    //initialize sensors board
    init_sensors();
    
    printf("DE-Nano Demo\r\nStart sending sensor data\n");
    
    // enble key interrupt
    EnableKeyInterrupt();  

    bool bSuccess = TRUE;
    while(bSuccess && !bKeyPressed){
        bSuccess = read_sensors();
        
        if (status_register & (1<<FLAG_RF_PTX_READY))
        {
            printf("%s",rf_ptx_buffer);
            
            status_register ^= (1<<FLAG_RF_PTX_READY);
        }
        //usleep(30000);
    }
    return 0;
    
       
}


void init_sensors(void){
    // set clock as output
    //IOWR_ALTERA_AVALON_PIO_DIRECTION(I2C_SCL_BASE, ALTERA_AVALON_PIO_DIRECTION_OUTPUT);
    IOWR(SELECT_I2C_CLK_BASE, 0, 0x01);
    // Sample Rate 1kHz, Filter Bandwidth 42Hz, Gyro Range 500 d/s
    if (!I2C_Write(I2C_SCL_BASE, I2C_SDA_BASE, GYRO, 0x16, 0x0B)) {printf("failed 1\n");return;}
    //set accel register data address
    if (!I2C_Write(I2C_SCL_BASE, I2C_SDA_BASE, GYRO, 0x18, 0x32)) printf("failed 2\n");
    // set accel i2c slave address
    if (!I2C_Write(I2C_SCL_BASE, I2C_SDA_BASE, GYRO, 0x14, ACCEL)) printf("failed 2\n");

    // Set passthrough mode to Accel so we can turn it on
    if (!I2C_Write(I2C_SCL_BASE, I2C_SDA_BASE, GYRO, 0x3D, 0x08)) printf("failed 4\n");
    // set accel power control to 'measure'
    if (!I2C_Write(I2C_SCL_BASE, I2C_SDA_BASE, ACCEL, ADXL345_POWER_CTL, 8)) printf("failed 5\n");
    //cancel pass through to accel, gyro will now read accel for us
    if (!I2C_Write(I2C_SCL_BASE, I2C_SDA_BASE, GYRO, 0x3D, 0x28)) printf("failed 6\n");
}
int read_sensors(void){
    //alt_u8 szBuf[12];
    int Num;
    //const alt_u8 DeviceAddr = 0xA0;
    //const alt_u8 ControlAddr = 00;
    
    
    alt_u8 buffer[12];   // Array to store ADC values 
    alt_16 gyro_x;
    alt_16 gyro_y;
    alt_16 gyro_z;
    alt_16 accel_x;
    alt_16 accel_y;
    alt_16 accel_z;
    int i;
    char to_rf[32];
    



        if (I2C_MultipleRead(I2C_SCL_BASE, I2C_SDA_BASE, GYRO, REG_GYRO_X, buffer, sizeof(buffer))){
            Num = sizeof(buffer)/sizeof(buffer[0]); 
            for(i=0;i<Num;i++){
            }
        }else{
            printf("Failed to access I2C\r\n");
            return FALSE;
        }
    
        //Combine bytes into integers
        // Gyro format is MSB first
        gyro_x = buffer[0] << 8 | buffer[1];
        gyro_y = buffer[2] << 8 | buffer[3];
        gyro_z = buffer[4] << 8 | buffer[5];
        // Accel is LSB first. Also because of orientation of chips
        // accel y output is in same orientation as gyro x
        // and accel x is gyro -y
        accel_y = buffer[7] << 8 | buffer[6];
        accel_x = buffer[9] << 8 | buffer[8];
        accel_z = buffer[11] << 8 | buffer[10];
    
        // Print out what we have
        /*int gyro_x_int = altu8_to_int(gyro_x);
        int gyro_y_int = altu8_to_int(gyro_y);
        int gyro_z_int = altu8_to_int(gyro_z);
        int accel_x_int = altu8_to_int(accel_x_int);
        int accel_y_int = altu8_to_int(accel_y_int);
        int accel_z_int = altu8_to_int(accel_z_int);*/
        sprintf(to_rf, "201:%d,%d,%d\n",gyro_x,gyro_y,gyro_z);
        rf_ptx_tx(to_rf);
        sprintf(to_rf,"202:%d,%d,%d\n",accel_x,accel_y,accel_z);
        rf_ptx_tx(to_rf);
        return TRUE;
        // echo the number received to screen
    
}
/*
void DEMO_EPCS(void){
    alt_u32 MemSize;
    MemSize = Flash_Size(EPCS_NAME);
    printf("EPCS Size:%d Bytes (%d MB)\r\n", (int)MemSize, (int)MemSize/1024/1024);
    
}

void DEMO_ACCELEROMETER(void){
    bool bSuccess;
    alt_16 szXYZ[3];
    alt_u8 id;
    const int mg_per_digi = 4;

    // release i2c pio pin
    //IOWR_ALTERA_AVALON_PIO_DIRECTION(I2C_SCL_BASE, ALTERA_AVALON_PIO_DIRECTION_OUTPUT);
    //IOWR_ALTERA_AVALON_PIO_DIRECTION(I2C_SDA_BASE, ALTERA_AVALON_PIO_DIRECTION_INPUT);
    IOWR(SELECT_I2C_CLK_BASE, 0, 0x00);

    // configure accelerometer as +-2g and start measure
    bSuccess = ADXL345_SPI_Init(GSENSOR_SPI_BASE);
    if (bSuccess){
        // dump chip id
        bSuccess = ADXL345_SPI_IdRead(GSENSOR_SPI_BASE, &id);
        if (bSuccess)
            printf("id=%02Xh\r\n", id);
    }

    if (bSuccess)
        printf("Monitor Accerometer Value. Press KEY0 or KEY1 to terminal the monitor process.\r\n");

    while(bSuccess && !bKeyPressed){
        if (ADXL345_SPI_IsDataReady(GSENSOR_SPI_BASE)){
            bSuccess = ADXL345_SPI_XYZ_Read(GSENSOR_SPI_BASE, szXYZ);
            if (bSuccess){
                printf("X=%d mg, Y=%d mg, Z=%d mg\r\n", szXYZ[0]*mg_per_digi, szXYZ[1]*mg_per_digi, szXYZ[2]*mg_per_digi);
                // show raw data,
                //printf("X=%04x, Y=%04x, Z=%04x\r\n", (alt_u16)szXYZ[0], (alt_u16)szXYZ[1],(alt_u16)szXYZ[2]);
                usleep(30000);
            }
        }
    }

    if (!bSuccess)
        printf("Failed to access accelerometer\r\n");

}

void DEMO_ADC(void){
    int ch = 0, next_ch=0;
    alt_u16 data16;

    printf("Monitor ADC Value. Press KEY0 or KEY1 to terminal the monitor process.\r\n");
    ADC_Read(next_ch);
    while(!bKeyPressed){
        next_ch++;
        if (next_ch >= 8)
            next_ch = 0;
        data16 = ADC_Read(next_ch); // 12-bits resolution
        printf("CH%d=%.2f V\r\n", ch, (float)data16 * 3.3 / 4095.0);
        ch = next_ch;

    }
}*/

